/******************************************************/
/* Goblin's Cube - Rubik's cube game for Wii Homebrew */
/* Copyright (C) 2009 Ricardo Bueno Cordeiro          */
/* Licensed under de GNU General Public License v2.0  */
/*   See license_gplv2.txt for detalis                */
/******************************************************/

#include <string.h>
#include <math.h>
#include <gccore.h>
#include <wiiuse/wpad.h>

#include "draw.h"
#include "control_game.h"
#include "model.h"

static Vector X = {1,0,0};
static Vector Y = {0,1,0};

typedef int table_entry_t[6];

static table_entry_t left_hold_rotation_table[] = {
	{  0,  1, -1,  3, -1,  2 }, // Front
	{ -1,  0,  1,  2,  3, -1 }, // Top 
	{  3, -1,  0, -1,  2,  1 }, // Right
	{ -1,  2,  1,  0,  3, -1 }, // Bottom
	{  1, -1,  2, -1,  0,  3 }, // Left
	{  2,  1, -1,  3, -1,  0 }  // Back
};
static table_entry_t right_hold_rotation_table[] = {
	{  0,  3, -1,  1, -1,  2 }, // Front
	{ -1,  0,  1,  2,  3, -1 }, // Top 
	{  3, -1,  0, -1,  2,  1 }, // Right
	{ -1,  2,  1,  0,  3, -1 }, // Bottom
	{  1, -1,  2, -1,  0,  3 }, // Left
	{  2,  3, -1,  1, -1,  0 }  // Back
};

ControlGame::ControlGame() {
	this->handness = RIGHT_HANDED;

	this->left.piece = PIECE_LEFT;
	this->left.operation = HAND_FREE;

	this->right.piece = PIECE_RIGHT;
	this->right.operation = HAND_FREE;
}

void ControlGame::get_wiimote_data(WPADData *data, u32 *buttons, struct gforce_t *gforce) {
	*buttons = 0x00000000;
	if (data->btns_h & WPAD_BUTTON_A)
		*buttons |= HAND_BUTTON_TURN;
	if (data->btns_h & WPAD_BUTTON_B)
		*buttons |= HAND_BUTTON_HOLD;
		
	*gforce = data->gforce;
}

void ControlGame::get_nunchuk_data(WPADData *data, u32 *buttons, struct gforce_t *gforce) {
	*buttons = 0x00000000;
	if (data->exp.nunchuk.btns_held & NUNCHUK_BUTTON_C)
		*buttons |= HAND_BUTTON_TURN;
	if (data->exp.nunchuk.btns_held & NUNCHUK_BUTTON_Z)
		*buttons |= HAND_BUTTON_HOLD;
		
	*gforce = data->exp.nunchuk.gforce;
	gforce->x /= 0.3;
	gforce->y /= 0.3;
	gforce->z /= 0.3;
}


void ControlGame::get_left_hand_data(WPADData *data, u32 *buttons, struct gforce_t *gforce) {
	if (this->handness == RIGHT_HANDED)
		this->get_nunchuk_data(data, buttons, gforce);
	else
		this->get_wiimote_data(data, buttons, gforce);
}

void ControlGame::get_right_hand_data(WPADData *data, u32 *buttons, struct gforce_t *gforce) {
	if (this->handness == RIGHT_HANDED)
		this->get_wiimote_data(data, buttons, gforce);
	else
		this->get_nunchuk_data(data, buttons, gforce);
}

int ControlGame::get_left_piece(struct gforce_t *gforce, int current) {
	if (fabs(gforce->x * gforce->x + gforce->y * gforce->y + gforce->z * gforce->z - 1.0) <= 0.2) {
		if (gforce->z >= 0.8)
			return PIECE_LEFT;
		if (gforce->z <= -0.8)
			return PIECE_RIGHT;
		if (gforce->x >= 0.8)
			return PIECE_TOP;
		if (gforce->x <= -0.8)
			return PIECE_BOTTOM;
		if (gforce->y >= 0.8)
			return PIECE_BACK;
		if (gforce->y <= -0.8)
			return PIECE_FRONT;
	}
	return current;
}

int ControlGame::get_right_piece(struct gforce_t *gforce, int current) {
	if (fabs(gforce->x * gforce->x + gforce->y * gforce->y + gforce->z * gforce->z - 1.0) <= 0.2) {
		if (gforce->z >= 0.8)
			return PIECE_RIGHT;
		if (gforce->z <= -0.8)
			return PIECE_LEFT;
		if (gforce->x >= 0.8)
			return PIECE_BOTTOM;
		if (gforce->x <= -0.8)
			return PIECE_TOP;
		if (gforce->y >= 0.8)
			return PIECE_BACK;
		if (gforce->y <= -0.8)
			return PIECE_FRONT;
	}
	return current;
}

void ControlGame::reset_left_rotation() {
	this->left.operation = HAND_HOLD;
	this->left.hold_position = this->left.piece;
}

void ControlGame::reset_right_rotation() {
	this->right.operation = HAND_HOLD;
	this->right.hold_position = this->right.piece;
}

void ControlGame::left_rotation_update(struct gforce_t *gforce) {
	int hold = this->get_left_piece(gforce, this->left.hold_position);

	if (this->left.hold_position == hold)
		return;

	int pre = left_hold_rotation_table[this->left.piece][this->left.hold_position];
	int pos = left_hold_rotation_table[this->left.piece][hold];

	if ((pre == -1) || (pos == -1))
		return;

	this->left.hold_position = hold;

	int direction = ((pos - pre + 5) % 4) - 1;

	if (direction == 0)
		return;
			
	if (((this->left.piece == PIECE_TOP) || (this->left.piece == PIECE_BOTTOM)) && ((this->angle > 0.0) || (this->angle < -180.0)))
		direction *= -1;

	if (direction < 0) {
		while (direction < 0) {
			Cube::cube.rotate_piece_cc(this->left.piece);
			direction++;
		}
	} else {
		while (direction > 0) {
			Cube::cube.rotate_piece_cw(this->left.piece);
			direction--;
		}
	}
}

void ControlGame::right_rotation_update(struct gforce_t *gforce) {
	int hold = this->get_right_piece(gforce, this->right.hold_position);

	if (this->right.hold_position == hold)
		return;

	int pre = right_hold_rotation_table[this->right.piece][this->right.hold_position];
	int pos = right_hold_rotation_table[this->right.piece][hold];

	if ((pre == -1) || (pos == -1))
		return;

	this->right.hold_position = hold;
	
	int direction = ((pos - pre + 5) % 4) - 1;

	if (direction == 0)
		return;
			
	if (((this->right.piece == PIECE_TOP) || (this->right.piece == PIECE_BOTTOM)) && ((this->angle > 0.0) || (this->angle < -180.0)))
		direction *= -1;
	
	if (direction < 0) {
		while (direction < 0) {
			Cube::cube.rotate_piece_cc(this->right.piece);
			direction++;
		}
	} else {
		while (direction > 0) {
			Cube::cube.rotate_piece_cw(this->right.piece);
			direction--;
		}
	}
}

int ControlGame::update(WPADData *wiimote_data, int state) {
	if (state == CONTROL_HIDDEN)
		return 0;

	u32 buttons;
	struct gforce_t	gforce;

	// Nunchuck orientation
	this->angle = 90.0 - wiimote_data->exp.nunchuk.js.ang;
	this->mag = wiimote_data->exp.nunchuk.js.mag;

	// Shortcuts to turn the cube
	if (wiimote_data->btns_d & WPAD_BUTTON_LEFT)
		Cube::cube.turn_right();
	if (wiimote_data->btns_d & WPAD_BUTTON_RIGHT)
		Cube::cube.turn_left();
	if (wiimote_data->btns_d & WPAD_BUTTON_UP)
		Cube::cube.turn_down();
	if (wiimote_data->btns_d & WPAD_BUTTON_DOWN)
		Cube::cube.turn_up();

	if (state == CONTROL_BACKGROUND)
		return 0;

	// Left hand
	this->get_left_hand_data(wiimote_data, &buttons, &gforce);
	if (buttons & HAND_BUTTON_HOLD) {
		if (this->left.operation != HAND_HOLD)
			this->reset_left_rotation();
		this->left_rotation_update(&gforce);
	} else {

		// When releasing the hold button, reset orientation
		if (this->left.operation == HAND_HOLD)
			this->left.piece = this->get_left_piece(&gforce, this->left.piece);

		// Compute current operation
		if (buttons & HAND_BUTTON_TURN)
			this->left.operation = HAND_TURN;
		else
			this->left.operation = HAND_FREE;

		// Look for change in orientation and turn cube if necessary
		int piece = this->get_left_piece(&gforce, this->left.piece);
		if ((this->left.operation == HAND_TURN) && (this->left.piece != piece))
			Cube::cube.turn(this->left.piece, piece);
		this->left.piece = piece;
	}

	// Right hand	
	this->get_right_hand_data(wiimote_data, &buttons, &gforce);
	if (buttons & HAND_BUTTON_HOLD) {
		if (this->right.operation != HAND_HOLD)
			this->reset_right_rotation();
		this->right_rotation_update(&gforce);
	} else {

		// When releasing the hold button, reset orientation
		if (this->right.operation == HAND_HOLD)
			this->right.piece = this->get_right_piece(&gforce, this->right.piece);

		// Compute current operation
		if (buttons & HAND_BUTTON_TURN)
			this->right.operation = HAND_TURN;
		else
			this->right.operation = HAND_FREE;

		// Look for change in orientation and turn cube if necessary
		int piece = this->get_right_piece(&gforce, this->right.piece);
		if ((this->right.operation == HAND_TURN) && (this->right.piece != piece))
			Cube::cube.turn(this->right.piece, piece);
		this->right.piece = piece;
	}
	
	// Undo
	if (wiimote_data->btns_d & WPAD_BUTTON_MINUS)
		Cube::cube.undo();

	// Resets and scramble the cube
	if (wiimote_data->btns_d & WPAD_BUTTON_1)
		Cube::cube.reset();
	if (wiimote_data->btns_d & WPAD_BUTTON_2)
		Cube::cube.scramble();

	return 0;
}

void ControlGame::get_orientation_mtx(Mtx m) {
	Mtx temp;

	f32 angle_cos = cos(DegToRad(this->angle));
	f32 angle_sin = sin(DegToRad(this->angle));

	guMtxIdentity(m);

	if (fabs(mag) >= 0.01) {
		guMtxRotAxisDeg(m, &X, angle_sin * this->mag * -40.0);
		guMtxIdentity(temp);
		guMtxRotAxisDeg(temp, &Y, angle_cos * this->mag * 40.0);
		guMtxConcat(m, temp, m);
	}
}

void ControlGame::draw_hand(int hand, f32 x, f32 y, f32 z, f32 r, f32 g, f32 b, f32 rotX, f32 rotY) {
	Mtx current, orientation, temp;

	DR_getCurrentModelMtx(current);

	guMtxIdentity(orientation);
	guMtxTransApply(orientation, orientation, x, y, z);

	guMtxIdentity(temp);
	guMtxScale(temp, 0.2, 0.2, 0.2);
	guMtxConcat(orientation, temp, orientation);

	guMtxRotAxisDeg(temp, &X, rotX);
	guMtxConcat(orientation, temp, orientation);
	guMtxRotAxisDeg(temp, &Y, rotY);
	guMtxConcat(orientation, temp, orientation);

	guMtxConcat(current, orientation, orientation);
	DR_setCurrentModelMtx(orientation);

	draw_model(hand, r, g, b);	

	DR_setCurrentModelMtx(current);
}

void ControlGame::draw_hands() {
	f32 x = 0.0, y = 0.0, z = 0.0;
	f32 rotX = 0.0, rotY = 0.0;
	f32 r, g, b;

	switch(this->left.piece) {
		case PIECE_LEFT:   x = -4.0; y = 1.0;  z = 0.0;  rotY = -90.0; break;
		case PIECE_TOP:    x = -1.0; y = 4.0;  z = 0.0;  rotX = -90.0; break;
		case PIECE_RIGHT:  x = 4.0;  y = -1.0; z = 0.0;  rotY =  90.0; break;
		case PIECE_BOTTOM: x = -1.0; y = -4.0; z = 0.0;  rotY = 180.0; rotX = -90.0; break;
		case PIECE_FRONT:  x = -2.0; y = 1.0;  z = 4.0; break;
		case PIECE_BACK:   x = -4.0; y = 1.0;  z = -4.0; rotY = 180.0; break;
	}
	
	switch(this->left.operation) {
		case HAND_TURN: r = 1.0; g = 1.0; b = 0.0; break;
		case HAND_HOLD: r = 1.0; g = 0.0; b = 0.0; break;
		default: r = 0.0; g = 1.0; b = 0.0;
	}
	
	GX_SetCullMode(GX_CULL_BACK);
	this->draw_hand(MODEL_LEFT_HAND, x, y, z, r, g, b, rotX, rotY);

	rotX = rotY = 0.0;

	switch(this->right.piece) {
		case PIECE_LEFT: x = -4.0; y = -1.0; z = 0.0; rotY = -90.0; break;
		case PIECE_TOP: x = 1.0; y = 4.0; z = 0.0; rotX = -90.0; break;
		case PIECE_RIGHT: x = 4.0; y = 1.0; z = 0.0; rotY = 90.0; break;
		case PIECE_BOTTOM: x = 1.0; y = -4.0; z = 0.0;  rotY = 180.0; rotX = -90.0; break;
		case PIECE_FRONT: x = 2.0; y = 1.0; z = 4.0; break;
		case PIECE_BACK: x = 4.0; y = 1.0; z = -4.0; rotY = 180.0; break;
	}
	
	switch(this->right.operation) {
		case HAND_TURN: r = 1.0; g = 1.0; b = 0.0; break;
		case HAND_HOLD: r = 1.0; g = 0.0; b = 0.0; break;
		default: r = 0.0; g = 1.0; b = 0.0;
	}

	GX_SetCullMode(GX_CULL_FRONT);
	this->draw_hand(MODEL_RIGHT_HAND, x, y, z, r, g, b, rotX, rotY);
}

void ControlGame::draw(int state) {
	if (state == CONTROL_HIDDEN)
		return;

	Mtx current, orientation;
	
	DR_getCurrentModelMtx(current);

	this->get_orientation_mtx(orientation);
	
	guMtxConcat(current, orientation, orientation);
	DR_setCurrentModelMtx(orientation);
	
	Cube::cube.draw();
	this->draw_hands();

	DR_setCurrentModelMtx(current);
}

void ControlGame::text(int state) {
	if (state == CONTROL_HIDDEN)
		return;

	DR_draw_text(50, 900, (GXColor){ 255, 255, 255, 255}, "Rotations: %d", Cube::cube.get_num_rotations());
	DR_draw_text(700, 900, (GXColor){ 255, 255, 255, 255}, "Time: %.2f", Cube::cube.get_current_time());
}
